/*
	AP_main.c

	contains functionality to initialise and manage Application Processors (APs)

	Author: Aidan Goddard 21/10/13
*/


#include"../general/driver_error_codes.h"
#include"../../kernel/headers/types.h"
#include"../../kernel/headers/system_config_tables.h"
#include"../../klib/headers/klib_proc_threads.h"

#include"../../setup/headers/printf.h"

extern void SetWorkingCPUCount(uint32_t count);

/*
	function to init the AP with the given APIC ID

	returns 0 on success, non-zero on error
*/
static uint32_t start_ap(uint32_t APIC_ID, volatile uint32_t *APIC_base)
{
	// generate the command fields for the INIT IPI and the SIPI IPI
	uint32_t command_high = APIC_ID << 24;
	uint32_t command_init = 0x00004500;
	uint32_t command_sipi = 0x00004602;

	// set the IPI address register
	__asm__ __volatile__("mfence":::"memory");
	APIC_base[0xc4] = command_high;
	__asm__ __volatile__("mfence":::"memory");

	// send the INIT IPI
	__asm__ __volatile__("mfence":::"memory");
	APIC_base[0xc0] = command_init;
	__asm__ __volatile__("mfence":::"memory");

	// wait 5ms
	__KLIB__SleepThisThread(5000);

	// set the IPI address register
	__asm__ __volatile__("mfence":::"memory");
	APIC_base[0xc4] = command_high;
	__asm__ __volatile__("mfence":::"memory");

	// send the first SIPI
	__asm__ __volatile__("mfence":::"memory");
	APIC_base[0xc0] = command_sipi;
	__asm__ __volatile__("mfence":::"memory");

	// wait 5ms
	__KLIB__SleepThisThread(5000);

	// check control variable for 1
	volatile uint32_t *check = (uint32_t*)(0x2100 - 4 + KOFFSET);
	volatile uint32_t *ready = (uint32_t*)(0x2100 - 12 + KOFFSET);

	if(*check == 1)
	{
		// AP has started correctly. Wait for it to finish and return success
		while(*ready == 0){}
		*ready = 0;
		*check = 0;

		return 0;
	}

	// hasn't started yet. send second SIPI
	// set the IPI address register
	__asm__ __volatile__("mfence":::"memory");
	APIC_base[0xc4] = command_high;
	__asm__ __volatile__("mfence":::"memory");

	// send the first SIPI
	__asm__ __volatile__("mfence":::"memory");
	APIC_base[0xc0] = command_sipi;
	__asm__ __volatile__("mfence":::"memory");

	// wait 5ms
	__KLIB__SleepThisThread(5000);

	// check control variable again
	if(*check == 1)
	{
		// AP has started correctly. Wait for it to finish and return success
		while(*ready == 0){}
		*ready = 0;
		*check = 0;

		return 0;
	}

	// AP did not initialise correctly, return error
	return 0xffffffff;
}

/*
	New function to initialise application processors (APs)

	Returns number of APs in system when all are successfully initialised.
	Returns error if unsuccessful.
*/

uint32_t AP_INIT(uint32_t *error)
{
	// copy trampoline code (4096) from 0x10000 to 0x2000
	uint8_t *ptr1 = (uint8_t*)(0x10000 + KOFFSET);
	uint8_t *ptr2 = (uint8_t*)( 0x2000 + KOFFSET);
	int i;
	for(i = 0; i < 4096; i++)
	{
		ptr2[i] = ptr1[i];
	}

	// get the sys info table ptr
	TBL_SYSCONFIG *config_base = GetSystemConfigBase();

	// get APIC base address
	uint32_t apic_low, apic_high;
	rdmsr(0x1b, &apic_low, &apic_high);		// get msr contents
	volatile uint32_t *APIC_base = (uint32_t*)(uint64_t)(apic_low & 0xfffff000);

	// for every CPU but the BSP
	for(i = 1; i < 32; i++)
	{
		// check it is enabled
		if (config_base->tbl_cpu[i].enabled == 0)
		{
			// last entry, leave
			break;
		}

		// get the APIC ID
		uint32_t APIC_ID = config_base->tbl_cpu[i].APIC_ID;

		// attempt to init the AP (5 attempts)
		int success = 0;
		int k;
		for(k = 0; k < 5; k++)
		{
			uint32_t e = start_ap(APIC_ID, APIC_base);
			if(e == 0)
			{
				success = 1;
				break;
			}
		}
		if(success == 0)
		{
			*error = APIC_ID;
			return 0xffffee00 + i;
		}


	}

	// get initialised count
	volatile uint32_t *count = (uint32_t*)(0x2100 - 8 + KOFFSET);
	//printf("\n Total CPUs in operation: %u", *count);

	// now tell the scheduler it may use the APs
	SetWorkingCPUCount(*count);

	if(*count == 1)
	{
		printf("\nWARNING: ONLY 1 CPU DETECTED AND INITIALISED");
		__KLIB__SleepThisThread(10000000);
	}

	// need to clear the identity page mapping in the kernel page table at 1MB
	// clear ID mapping for low 512 MB
	uint64_t *k_page_map = (uint64_t*)(KOFFSET + 0x102000);
	for(i = 0; i < 256; i++)
	{
		// zero the entry
		k_page_map[i] = 0;

		// invalidate it
		uint64_t address_to_invalidate = i * 0x200000;
		__asm__ __volatile__("invlpg %0" :: "m"(address_to_invalidate) : "memory");
	}

	// return success
	return DRIVER_STARTUP_SUCCESS;
}

















